/*******************************************************************************
* This file implements the navigation algorithm for the UAV
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#ifndef _NAVIGATION_H
#define _NAVIGATION_H



#include <math.h>
#include "FreeRTOS.h"
#include "task.h"
#include "system.h"
#include "hal_gps.h"



/*******************************************************************************
* ENUM, STRUCTURE AND UNION DEFINITION                                         *
*******************************************************************************/

// Navigation command type.
enum NAV_COMMAND_TYPE {
	GOTO,
	GOTO_FROM,
	LOITER_COUNT,
	LOITER_TIME,
};	



struct NAV_COMMAND{
	enum NAV_COMMAND_TYPE eCommand;
	
	union {
		struct {
			float fLatitude;
			float fLongitude;
			unsigned int uiAltitude;
		} xGotoParam;
		
		struct {
			float fLatitude;
			float fLongitude;
			unsigned int uiAltitude;
			float fFromLatitude;
			float fFromLongitude;
		} xGotoFromParam;
		
		struct {
			float fLatitude;
			float fLongitude;
			unsigned int uiAltitude;
			unsigned int uiRadius;
			signed int siLoiterDir;
			unsigned int uiCount;
		} xLoiterCountParam;
		
		struct LOITER_TIME_PARAM {
			float fLatitude;
			float fLongitude;
			unsigned int uiAltitude;
			unsigned int uiRadius;
			signed int siLoiterDir;
			unsigned int uiTime;
		} xLoiterTimeParam;
	};	
};



/*******************************************************************************
* PUBLIC FUNCTION PROTOTYPES                                                   *
*******************************************************************************/

/*******************************************************************************
* PUBLIC FUNCTION: fGetHeadingSetpoint
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The heading setpoint (0 - 2PI rad).
*
* DESCRIPTION:
* Get the heading setpoint of the UAV.
*
*******************************************************************************/
float fGetHeadingSetpoint(void);



/*******************************************************************************
* PUBLIC FUNCTION: fGetBearingToTarget
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The bearing to target (0 - 2PI rad).
*
* DESCRIPTION:
* Get the bearing from the current position to the target.
*
*******************************************************************************/
float fGetBearingToTarget(void);



/*******************************************************************************
* PUBLIC FUNCTION: fGetDistanceToTarget
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The distance to target (m).
*
* DESCRIPTION:
* Get the distance from the current position to the target.
*
*******************************************************************************/
float fGetDistanceToTarget(void);



/*******************************************************************************
* TASK: taskNavigationLoop
*
* DESCRIPTIONS:
* This task is the main loop for the navigation calculation.
*
*******************************************************************************/
portTASK_FUNCTION_PROTO(taskNavigationLoop, pvParameters);



#endif
